#include<hikari.h>
#include "BulletPhysicsHComponent.h"
#include <BulletCollision/Gimpact/btGImpactShape.h>
#include <BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h>


btVector3 vectorToBtVector(const irr::core::vector3df&vector);
irr::core::vector3df btVectorToVector(const btVector3&v);

inline static irr::core::vector3df bulletTransformToIrrRotation(const btTransform & tr)
{
	irr::core::matrix4 mat;
	irr::f32 * ptr;
	ptr = mat.pointer();
	tr.getOpenGLMatrix(ptr);
	return mat.getRotationDegrees();
}
inline static btTransform irrRotationToBulletTransform(const irr::core::vector3df & rotation)
{
	irr::core::matrix4 mat;
	mat.setRotationDegrees(rotation);
	btTransform tr;
	tr.setFromOpenGLMatrix(mat.pointer());

	return tr;
}

static btTransform getTransformFromIrrlichtNode(const irr::scene::ISceneNode * irrNode)
{
	btTransform transf;
	transf.setIdentity();
	transf.setOrigin(vectorToBtVector(irrNode->getPosition()));
	transf.getBasis().setEulerZYX(irrNode->getRotation().X * irr::core::DEGTORAD,irrNode->getRotation().Y* irr::core::DEGTORAD,irrNode->getRotation().Z* irr::core::DEGTORAD);
	return transf;
}


btVector3 vectorToBtVector(const irr::core::vector3df&vector){
	return btVector3(vector.X, vector.Y, vector.Z);
}

irr::core::vector3df btVectorToVector(const btVector3&v){
	return irr::core::vector3df(v.getX(), v.getY(), v.getZ());
}

static btTriangleMesh* irrMeshToBulletTriangleMesh(irr::scene::IMesh* pMesh,const irr::core::vector3df& scaling){
	btVector3 vertices[3];
	irr::u32 i,j,k,index,numVertices,numIndices;
	irr::u16* mb_indices;
	btTriangleMesh *pTriMesh = new btTriangleMesh();
	for (i=0; i<pMesh->getMeshBufferCount(); i++){
		irr::scene::IMeshBuffer* mb=pMesh->getMeshBuffer(i);
		if(mb->getVertexType()==irr::video::EVT_STANDARD){
			irr::video::S3DVertex* mb_vertices=(irr::video::S3DVertex*)mb->getVertices();
			mb_indices = mb->getIndices();
			numVertices = mb->getVertexCount();
			numIndices = mb->getIndexCount();
			for(j=0;j<numIndices;j+=3){
				for (k=0;k<3;k++){
					index = mb_indices[j+k];
					vertices[k] = btVector3(mb_vertices[index].Pos.X*scaling.X, mb_vertices[index].Pos.Y*scaling.Y, mb_vertices[index].Pos.Z*scaling.Z);
				}

				pTriMesh->addTriangle(vertices[0], vertices[1], vertices[2]);
			}
		}else if(mb->getVertexType()==irr::video::EVT_2TCOORDS){
			irr::video::S3DVertex2TCoords* mb_vertices=(irr::video::S3DVertex2TCoords*)mb->getVertices();
			mb_indices = mb->getIndices();
			numVertices = mb->getVertexCount();
			numIndices = mb->getIndexCount();
			for(j=0;j<numIndices;j+=3){
				for (k=0;k<3;k++){
					index = mb_indices[j+k];
					vertices[k] = btVector3(mb_vertices[index].Pos.X*scaling.X, mb_vertices[index].Pos.Y*scaling.Y, mb_vertices[index].Pos.Z*scaling.Z);
				}

				pTriMesh->addTriangle(vertices[0], vertices[1], vertices[2]);
			}
		}
	}

	return pTriMesh;
}

btCollisionShape*createBoxRigidShape(FileSection*fileSection,HObj*object){
	vector3df size=fileSection->readVector(fileSection->getChunk(L"size"));
	btVector3 halfExtents(size.X, size.Y , size.Z);
	btCollisionShape *shape = new btBoxShape(halfExtents);
	delete fileSection;
	return shape;
}

btCollisionShape*createTriMeshRigidShape(FileSection*fileSection,HObj*object){
	//btCollisionShape *shape = new btBoxShape(halfExtents);
	btTriangleMesh*m_indexVertexArrays = irrMeshToBulletTriangleMesh(object->getMesh(),object->getScale());
	btBvhTriangleMeshShape*m_trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays, true);
	//m_trimeshShape->updateBound();
	delete fileSection;
	return m_trimeshShape;
}

btCollisionShape*createSphereRigidShape(FileSection*fileSection,HObj*object){
	float radius=fileSection->readFloat(fileSection->getChunk(L"radius"));
	btCollisionShape *shape = new btSphereShape(radius);
	delete fileSection;
	fileSection=NULL;
	return shape;
}

/*btCollisionShape*createCylinderRigidShape(FileSection*fileSection,HObj*object){
	float radius=fileSection->readFloat(fileSection->getChunk(L"radius"));
	//btCylinderShapeData
	//btCylinderShape *shape = new btCylinderShape();
	delete fileSection;
	return shape;
}*/

BulletPhysicsHComponent*readRigidBody(FileSection*section,HObj*object){
	btCollisionShape*shape=NULL;
	FileSection*rigidBodySection=section->readSection(section->getChunk(L"rigidBody"));
	vector<Chunk*> sections=rigidBodySection->getAllSections();
	bool calculateLocalInertia=true;
	for(unsigned int i=0;i<sections.size();i++){
		if(Utils::equal(sections[i]->getName(),L"boxRigid")){
			shape=createBoxRigidShape(section->readSection(sections[0]), object);
			break;
		}
		if(Utils::equal(sections[i]->getName(),L"sphereRigid")){
			shape=createSphereRigidShape(section->readSection(sections[0]), object);
			break;
		}
		if(Utils::equal(sections[i]->getName(),L"trimeshRigid")){
			shape=createTriMeshRigidShape(section->readSection(sections[0]), object);
			calculateLocalInertia=false;
			break;
		}

		Utils::errorLog(wstring(L"Cannot find rigidbody shape "+sections[i]->getName()));
		return NULL;
	}

	delete rigidBodySection;
	rigidBodySection=NULL;
	float mass=section->readFloat(section->getChunk(L"mass"));
	float restitution=section->readFloat(section->getChunk(L"restitut"));
	float friction=section->readFloat(section->getChunk(L"frict"));
	float rollingFriction=section->readFloat(section->getChunk(L"rollingFrict"));
	float ccdMotionThreshold=section->readFloat(section->getChunk(L"motThres"));
	float contactProcessingThreshold=section->readFloat(section->getChunk(L"contProcThr"));
	float sweptRadius=section->readFloat(section->getChunk(L"sweptradius"));
	float linearDamping=section->readFloat(section->getChunk(L"linearDamp"));
	float angDamping=section->readFloat(section->getChunk(L"angDamp"));
	float deactivationTime=section->readFloat(section->getChunk(L"deactTime"));
	float hitFraction=section->readFloat(section->getChunk(L"hitfract"));
	vector3df linearVelocity=section->readVector(section->getChunk(L"linearVelocity"));
	vector3df linearFactor=section->readVector(section->getChunk(L"linearFactor"));
	vector3df angularFactor=section->readVector(section->getChunk(L"angularFactor"));
	vector3df angularVelocity=section->readVector(section->getChunk(L"angularVelocity"));
	vector3df anisotropicFriction=section->readVector(section->getChunk(L"anisFriction"));
	vector3df offsetPos=section->readVector(section->getChunk(L"offsetPos"));
	vector3df offsetRot=section->readVector(section->getChunk(L"offsetRot"));
	bool disableDeactivation=section->readBoolean(section->getChunk(L"disableDeact"));
	bool isChar=section->readBoolean(section->getChunk(L"isChar"));
	bool isKinemat=section->readBoolean(section->getChunk(L"isKinemat"));
	bool isStatic=section->readBoolean(section->getChunk(L"isStatic"));

	btTransform Transform;
	Transform.setIdentity();
	Transform.setOrigin(vectorToBtVector(object->getPosition()+offsetPos));

	btDefaultMotionState *motionState = new btDefaultMotionState(Transform);
	btVector3 localInertia;
	if(calculateLocalInertia==true){
		shape->calculateLocalInertia(mass, localInertia);
	}
	btRigidBody *rigidBody = new btRigidBody(mass, motionState, shape, localInertia);
	rigidBody->setRestitution(restitution);
	rigidBody->setFriction(friction);
	rigidBody->setRollingFriction(rollingFriction);
	rigidBody->setCcdMotionThreshold(ccdMotionThreshold);
	rigidBody->setContactProcessingThreshold(contactProcessingThreshold);
	rigidBody->setCcdSweptSphereRadius(sweptRadius);
	rigidBody->setDamping(linearDamping, angDamping);
	rigidBody->setDeactivationTime(deactivationTime);
	rigidBody->setHitFraction(hitFraction);
	rigidBody->setLinearVelocity(vectorToBtVector(linearVelocity));
	rigidBody->setLinearFactor(vectorToBtVector(linearFactor));
	rigidBody->setAngularFactor(vectorToBtVector(angularFactor));
	rigidBody->setAngularVelocity(vectorToBtVector(angularVelocity));
	rigidBody->setAnisotropicFriction(vectorToBtVector(anisotropicFriction));
	rigidBody->setGravity(btVector3(0,1,0));
	if(disableDeactivation){
		rigidBody->setActivationState(DISABLE_DEACTIVATION);
	}
	if(isKinemat){
		rigidBody->setCollisionFlags( rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
	}
	if(isChar){
		rigidBody->setCollisionFlags( rigidBody->getCollisionFlags() | btCollisionObject::CF_CHARACTER_OBJECT);
	}
	if(isStatic){
		rigidBody->setCollisionFlags( rigidBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
	}

	vector3df rotation=object->getRotation();
	btTransform bt;
	bt = irrRotationToBulletTransform(rotation);
	bt.setOrigin(rigidBody->getWorldTransform().getOrigin());
	rigidBody->setWorldTransform(bt);

	BulletPhysicsHComponent*component=new BulletPhysicsHComponent(object, rigidBody, offsetPos,offsetRot);
	return component;
}

HObj* BulletPhysLoadHandler::read(HikariContext*context, HObj*object, FileSection*section){
	//FileSection*rigidBodySection=section->readSection(section);
	BulletPhysicsHComponent*component=readRigidBody(section, object);
	component->setName(L"bulletPhysics");
	//delete rigidBodySection;
	object->setRigidBody(component->getRigidBody());
	object->addRenderComponent(component);
	context->getCollisionWorld()->addRigidBody(component->getRigidBody(),1,1);
	return object;
}